#include "serial/robot_base_node.hpp"

#include <fmt/color.h>
#include <fmt/core.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float32.h>

namespace serial {

RobotBaseNode::RobotBaseNode()
    : nh_(),
      mode_(0),
      first_recv_(true),
      first_send_(true),
      bullet_speed_(20),
      yaw_(0),
      pitch_(0) {
  std::string port_name;
  nh_.param("serial/port_name", port_name, std::string("/dev/ttyUSB0"));
  auto transporter = std::make_shared<serial::UartTransporter>(port_name);
  packet_tool_ = std::make_shared<FixedPacketTool<16>>(transporter);

  sub_ = nh_.subscribe<rm_interfaces::SerialSendMsg>(
      "serial_sendmsg", 100, &RobotBaseNode::gimbal_cmd_cb, this);
  pub_ = nh_.advertise<std_msgs::Float32>("/serial_receive", 1);
  set_mode_client_ =
      nh_.serviceClient<rm_interfaces::SetMode>("/detector/set_mode");
  listen_thread_ =
      std::make_unique<std::thread>(&RobotBaseNode::listen_loop, this);
}

void RobotBaseNode::gimbal_cmd_cb(
    const rm_interfaces::SerialSendMsgConstPtr &msg) {
  if (first_send_) {
    fmt::print(
        fg(fmt::color::green) | fmt::emphasis::italic | fmt::emphasis::bold,
        "程序已启动\n");
    first_send_ = false;
  }
  FixedPacket<16> packet;
  packet.load_data<unsigned char>(0x00, 1);
  packet.load_data<float>(msg->pitch, 2);
  packet.load_data<float>(msg->yaw, 6);
  packet.load_data<float>(msg->distance, 10);
  // packet.set_check_byte();
  packet_tool_->send_packet(packet);
}

void RobotBaseNode::listen_loop() {
  ros::Rate frequency(200);
  FixedPacket<16> packet;
  int mode = mode_;
  float yaw;
  float pitch;
  float bullet_speed;
  while (ros::ok()) {
    if (packet_tool_->recv_packet(packet)) {
      unsigned char mode_byte;
      packet.unload_data(mode_byte, 1);
      mode = static_cast<int>(mode_byte);
      packet.unload_data(bullet_speed, 2);
      packet.unload_data(pitch, 6);
      packet.unload_data(yaw, 10);

      if (first_recv_ || mode_ != mode) {
        rm_interfaces::SetMode set_mode_srv;
        set_mode_srv.request.mode = mode;
        if (set_mode_client_.call(set_mode_srv)) {
          mode_ = mode;
        }
        first_recv_ = false;
      }
    }
    sensor_msgs::JointState joint_state;
    joint_state.header.stamp = ros::Time::now();
    joint_state.name.emplace_back("joint_yaw");
    joint_state.name.emplace_back("joint_pitch");
    joint_state.position.push_back(yaw);
    joint_state.position.push_back(-pitch);
    yaw_ = yaw;
    pitch_ = pitch;
    joint_state_pub_.publish(joint_state);

    if (bullet_speed > 10 && bullet_speed < 30) {
      bullet_speed_ = bullet_speed * 0.5 + bullet_speed_ * 0.5;
    }
    rm_interfaces::SerialReceiveMsg serial_msg;
    serial_msg.yaw = yaw;
    serial_msg.pitch = -pitch;
    bullet_speed_pub_.publish(serial_msg);
    frequency.sleep();
  }
}

}  // namespace serial
